#include "padding.h"
#include <string.h>

unsigned int AddPKCS7Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    unsigned int newLen;
    newLen = data_len + block_size - (data_len % block_size);
    memset(&data[data_len], newLen-data_len, newLen-data_len);
    return newLen;
}

unsigned int DelPKCS7Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    if ((data_len%block_size)!=0 || !data_len || data[data_len-1]>data_len) return 0;
    return data_len - data[data_len - 1];
}

unsigned int AddZeroPadding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    unsigned int newLen;
    newLen = data_len + block_size - (data_len % block_size);
    memset(&data[data_len], 0, newLen-data_len);
    return newLen;
}

unsigned int DelZeroPadding(unsigned char *data, unsigned int data_len, unsigned char block_size) {    
    if ((data_len%block_size)!=0 || !data_len) return 0;

    while(data_len && data[data_len-1]==0) data_len--;
    return data_len;
}

unsigned int AddISO10126Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    unsigned int newLen;
    newLen = data_len + block_size - (data_len % block_size);
    // memset(&data[data_len], 11, newLen-data_len);     
    data[newLen-1] = newLen-data_len;
    return newLen;
}

unsigned int DelISO10126Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {    
    if ((data_len%block_size)!=0 || 
        !data_len || 
        data[data_len-1]==0 || 
        data[data_len-1]>block_size ||
        data[data_len-1]>=data_len) {
        return 0;
    }

    return data_len - data[data_len-1];
}

unsigned int AddISO7816Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    unsigned int newLen;
    newLen = data_len + block_size - (data_len % block_size);
    *(data+data_len) = 0x80;
    memset(data+data_len+1, 0, newLen-data_len-1);
    return newLen;
}

unsigned int DelISO7816Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {    
    if ((data_len%block_size)!=0 || !data_len) return 0;

    while(data_len && data[data_len-1]!=0x80) {
        data_len--;
    }

    return data_len?data_len-1:0;
}

unsigned int AddANSIX923Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    unsigned int newLen;
    newLen = data_len + block_size - (data_len % block_size);
    memset(&data[data_len], 0, newLen-data_len);
    data[newLen-1] = newLen-data_len;
    return newLen;
}

unsigned int DelANSIX923Padding(unsigned char *data, unsigned int data_len, unsigned char block_size) {
    if ((data_len%block_size)!=0 || !data_len || data[data_len-1]>data_len) return 0;
    return data_len - data[data_len - 1];
}
